/******************************************************************************
 *   Different utilitary helper functions for Command module and Create
 ******************************************************************************/

#ifndef _OWN_UTILS_H_
#define _OWN_UTILS_H_

#include "oi.h"
//#include <cstdlib>

void ledsInit(void);
void ledsSet(int8_t x);
void serialTx(uint8_t value);
uint8_t serialRx(void);
void serialWait(void);
void baud(uint8_t baudCode);
void delayMs(uint16_t timeMs);
void powerOnCreate(void);

void servoRotate(int8_t degree);


/******************************************************************************
 * Timer global variables
 ******************************************************************************/
volatile uint16_t timerCnt = 0;
volatile uint8_t timerOn = 0;


/******************************************************************************
 * Initialize I/O ports for LEDs
 ******************************************************************************/
void ledsInit(void) 
{
    DDRB |= 0x0F; // 00001111b
    PORTB &= 0xF0;
    DDRC |= 0x0F; // 00001111b
    PORTC &= 0xF0;
}

/******************************************************************************
 * Turns on LED for each non-zero bit in argument.
 * Lower 4 bits goes through B0-B3, higher 4 - through C0-C3
 ******************************************************************************/
void ledsSet(int8_t x) 
{
    PORTB &= 0xF2;
    PORTB |= (x & 0x0D);
    PORTC &= 0xF0;
    PORTC |= (x >> 4);
}

/******************************************************************************
 * Transmits byte over the serial port
 ******************************************************************************/
void serialTx(uint8_t value)
{
    while (!(UCSR0A & 0x20)) ;
    UDR0 = value;
}

/******************************************************************************
 * Receives byte from the serial port
 ******************************************************************************/
uint8_t serialRx(void)
{
    while(!(UCSR0A & 0x80)) ;
    return UDR0;
}

/******************************************************************************
 * Waits until transmission over serial port is finished
 ******************************************************************************/
void serialWait(void)
{
    while(!(UCSR0A & _BV(TXC0))) ;
}

/******************************************************************************
 * Switch the baud rate on both Create and module
 ******************************************************************************/
void baud(uint8_t baudCode)
{
  if(baudCode <= 11)
  {
    serialTx(CmdBaud);
    UCSR0A |= _BV(TXC0);
    serialTx(baudCode);
    // Wait until transmit is complete
    while(!(UCSR0A & _BV(TXC0))) ;

    cli();

    // Switch the baud rate register
    if(baudCode == Baud115200) {
        UBRR0 = Ubrr115200;
    } else if(baudCode == Baud57600) {
        UBRR0 = Ubrr57600;
    } else if(baudCode == Baud38400) {
        UBRR0 = Ubrr38400;
    } else if(baudCode == Baud28800) {
        UBRR0 = Ubrr28800;
    } else if(baudCode == Baud19200) {
        UBRR0 = Ubrr19200;
    } else if(baudCode == Baud14400) {
        UBRR0 = Ubrr14400;
    } else if(baudCode == Baud9600) {
        UBRR0 = Ubrr9600;
    } else if(baudCode == Baud4800) {
        UBRR0 = Ubrr4800;
    } else if(baudCode == Baud2400) {
        UBRR0 = Ubrr2400;
    } else if(baudCode == Baud1200) {
        UBRR0 = Ubrr1200;
    } else if(baudCode == Baud600) {
        UBRR0 = Ubrr600;
    } else if(baudCode == Baud300) {
        UBRR0 = Ubrr300;
    }
    sei();

    delayMs(100);
  }
}

/******************************************************************************
 * Delay for the specified time in ms without updating sensor values
 ******************************************************************************/
void delayMs(uint16_t timeMs)
{
  timerOn = 1;
  timerCnt = timeMs;
  while(timerOn) ;
}

/******************************************************************************
 * Timer 0 interrupt to time delays in ms
 ******************************************************************************/
SIGNAL(SIG_OUTPUT_COMPARE0A)
{
    if (timerOn) {
        if(timerCnt) {
            timerCnt--;
        } else {
            timerOn = 0;
        }
    }
}

/******************************************************************************
 * Powers iRobot Create on
 ******************************************************************************/
void powerOnCreate(void)
{
    DDRB &= ~0x20;
    PORTB &= ~0x20;
    DDRD |= 0x80;
    delayMs(2000);
    if (!(PINB & 0x20)) {
        PORTD &= ~0x80;
        delayMs(100);
        PORTD |= 0x80;
        delayMs(100);
        delayMs(3000);
    }
}

void servoRotate(int8_t degree)
{
    OCR1A = (135 + (int16_t)degree)*64 / 5;
}


/******************************************************************************
 * Serial debugging functions
 ******************************************************************************/
#define UART_TO_USB 1
#define UART_TO_CREATE 2

void setSerialDestination(uint8_t com)
{
    if (com == UART_TO_USB) {
        PORTB |= 0x10;
    } else if (com == UART_TO_CREATE) {
        PORTB &= ~0x10;
    }
}

uint8_t getSerialDestination(void) 
{
    if (PORTB & 0x10) {
        return UART_TO_USB;
    } else {
        return UART_TO_CREATE;
    }
}

void debugWriteChar(char c, uint8_t com) 
{
    uint8_t origDest = getSerialDestination();
    
    if (com != origDest) {
        setSerialDestination(com);
        delayMs(200);
    }
    serialTx((uint8_t)(c));
    if (com != origDest) {
        delayMs(50);
        setSerialDestination(origDest);
        delayMs(200);
    }
}

void debugWriteString(char* s, uint8_t com) 
{
    uint8_t origDest = getSerialDestination();
    
    if (com != origDest) {
        setSerialDestination(com);
        delayMs(200);
    }
    uint8_t p = 0;
    while (s[p]) {
        serialTx((uint8_t)s[p]);
        ++p;
    }
    if (com != origDest) {
        delayMs(50);
        setSerialDestination(origDest);
        delayMs(200);
    }
}

void debugWriteInt(uint16_t v, uint8_t com) 
{
    char s[10];
    debugWriteString(itoa(v, s, 10), com);
}

#endif 
